// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
// Refer to README.md in this directory.

#pragma once
#include "Kalman.h"
#include "HungarianAlg.h"
#include "defines.h"
#include <iostream>
#include <vector>
#include <memory>
#include <array>

// --------------------------------------------------------------------------
class CTrack
{
public:
    CTrack(const Point_t &p, const std::vector<cv::Point> &contour, track_t dt, size_t trackID)
            : track_id(trackID), skipped_frames(0), prediction(p), lastContour(contour), KF(p, dt) {}

    track_t CalcDist(const Point_t &p)
    {
        Point_t diff = prediction - p;
        return std::sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
    }

    void Update(const Point_t &p, const std::vector<cv::Point> &contour, bool dataCorrect, size_t max_trace_length)
    {
        KF.Prediction();
        prediction = KF.Update(p, dataCorrect);
        if (dataCorrect) lastContour = contour;
        if (trace.size() > max_trace_length) trace.erase(trace.begin(), trace.end() - max_trace_length);
        trace.push_back(prediction);
    }

    // Returns contour in [px], not in [m]!
    std::vector<cv::Point> getLastContour() const { return lastContour; }

    // Returns velocity in [px/s], not in [m/s]!
    Point_t getEstimatedVelocity() const { return KF.LastVelocity; }

    std::vector<Point_t> trace;
    size_t track_id;
    size_t skipped_frames;
private:
    Point_t prediction;
    // Contour liegt immer auf ganzen Pixeln -> Integer Punkt
    std::vector<cv::Point> lastContour;
    TKalmanFilter KF;
};

// --------------------------------------------------------------------------
class CTracker
{
public:
    struct Params
    {
        // time for one step of the filter  过滤器执行一步的时间
        track_t dt;
        // distance threshold. Pairs of points with higher distance are not considered in the assignment problem
        // 距离阈值。在分配问题中不考虑距离较大的点对
        track_t dist_thresh;
        // Maximum number of frames the track is maintained without seeing the object in the measurement data
        // 在测量数据中看不到对象的情况下保持轨道的最大帧数
        int max_allowed_skipped_frames;
        // Maximum trace length  最大跟踪长度
        int max_trace_length;
    };

    CTracker(const Params &parameters);

    ~CTracker(void);

    std::vector<std::unique_ptr<CTrack>> tracks;

    void Update(const std::vector<Point_t> &detectedCentroid, const std::vector<std::vector<cv::Point> > &contour);

    void updateParameters(const Params &parameters);

private:
    // Aggregated parameters for the tracker object 跟踪器对象的聚合参数
    Params params;
    // ID for the upcoming CTrack object 即将到来的CTrack对象的ID
    size_t NextTrackID;
};
